package scu.maqiang.fes;

import static scu.maqiang.numeric.MVO.*;

import java.util.Arrays;
import scu.maqiang.mesh.*;
import scu.maqiang.numeric.CG;
import scu.maqiang.numeric.MVO;
import scu.maqiang.numeric.ParamCheck;
import scu.maqiang.numeric.SRMatrix;

public class FES3T42 extends FES {
	public FES3T42(Mesh3T4 mesh2D) {
		super(mesh2D, 3);
		eleStiff = FES3T42::elementElasticityStiff;
		eleMass = FES3T42::elementElasticityMass;
		eleSource = FES3T42::elementVolumeForce;
		eleFlux = FES3T42::elementSurfaceForce;
	}
	/**
	 * ���㵯�����ⵥԪ�նȾ���
	 *
	 * @param ele
	 * @param xyz
	 *            3x2ά��Ԫ�������ÿ�б�ʾ����x��y����
	 * @param ENu
	 *            2x3ά���Գ���
	 * @param tp
	 * @param K
	 */
	public static void elementElasticityStiff(int[] ele, double[][] xyz, double[][] ENu, BVPType tp, double[][] K) {
		
		double[][] grad = new double[4][3];
		double volume = CG.gradient3DT4(xyz, grad);
		double[] E = ENu[0];
		double[] Nu = ENu[1];
		double mE = average(E);
		double mNu = average(Nu);
		double[][] BK = new double[6][12];
//		BK[0][0] = BK[3][1] = BK[5][2] = grad[0][0];
//		BK[1][1] = BK[3][0] = BK[4][2] = grad[0][1];
//		BK[2][2] = BK[4][1] = BK[5][0] = grad[0][2];
//
//		BK[0][3] = BK[3][4] = BK[5][5] = grad[1][0];
//		BK[1][4] = BK[3][3] = BK[4][5] = grad[1][1];
//		BK[2][5] = BK[4][4] = BK[5][3] = grad[1][2];
//
//		BK[0][6] = BK[3][7] = BK[5][8] = grad[2][0];
//		BK[1][7] = BK[3][6] = BK[4][8] = grad[2][1];
//		BK[2][8] = BK[4][7] = BK[5][6] = grad[2][2];
//
//		BK[0][9] = BK[3][10] = BK[5][11] = grad[3][0];
//		BK[1][10] = BK[3][9] = BK[4][11] = grad[3][1];
//		BK[2][11] = BK[4][10] = BK[5][9] = grad[3][2];
		for(int i = 0; i < 4; i++) {
			BK[0][3 * i]     = BK[3][3 * i + 1] = BK[5][3 * i + 2] = grad[i][0];
			BK[1][3 * i + 1] = BK[3][3 * i]     = BK[4][3 * i + 2] = grad[i][1];
			BK[2][3 * i + 2] = BK[4][3 * i + 1] = BK[5][3 * i]     = grad[i][2];
		}
		MVO.fill(K, 0.0);
		switch (tp) {
		case COMMON:
			double[][] D = Materials.ElasticConstituteMatrix(mE, mNu, tp);
			mul(matmul(transpose(BK), matmul(D, BK)), volume, K);
			break;
		case CONSTITUTE_MATRIX_COMMON:
			mul(matmul(transpose(BK), matmul(ENu, BK)), volume, K);
			break;
		default:
			throw new IllegalArgumentException("Wrong BVPType!");
		}
	}

	public static void elementElasticityMass(int[] ele, double[][] xyz, double[][] r, BVPType tp, double[][] K) {
		double[] rho = r[0];
		double[][] grad = new double[4][3];
		double volume = CG.gradient3DT4(xyz, grad);
		MVO.fill(K, 0.0);
	        if(tp == BVPType.CONSIST_MASS) {
	        	K[0][0] = K[1][1] = K[2][2] = (3 * rho[0] + rho[1] + rho[2] + rho[3]) / 60 * volume;
	        	K[0][3] = K[1][4] = K[2][5] = (2 * rho[0] + 2 * rho[1] + rho[2] + rho[3]) / 120 * volume;
	        	K[3][0] = K[4][1] = K[5][2] = (2 * rho[0] + 2 * rho[1] + rho[2] + rho[3]) / 120 * volume;
	        	K[0][6] = K[1][7] = K[2][8] = (2 * rho[0] + rho[1] + 2 * rho[2] + rho[3]) / 120 * volume;
	        	K[6][0] = K[7][1] = K[8][2] = (2 * rho[0] + rho[1] + 2 * rho[2] + rho[3]) / 120 * volume;
	        	K[0][9] = K[1][10] = K[2][11] = (2 * rho[0] + rho[1] + rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[9][0] = K[10][1] = K[11][2] = (2 * rho[0] + rho[1] + rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[3][3] = K[4][4] = K[5][5] =   (rho[0] + 3 * rho[1] + rho[2] + rho[3]) / 60 * volume;
	        	K[3][6] = K[4][7] = K[5][8] =   (rho[0] + 2 * rho[1] + 2 * rho[2] + rho[3]) / 120 * volume;
	        	K[6][3] = K[7][4] = K[8][5] =   (rho[0] + 2 * rho[1] + 2 * rho[2] + rho[3]) / 120 * volume;
	        	K[3][9] = K[4][10] = K[5][11] = (rho[0] + 2 * rho[1] + rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[9][3] = K[10][4] = K[11][5] = (rho[0] + 2 * rho[1] + rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[6][6] = K[7][7] = K[8][8] = (rho[0] + rho[1] + 3 * rho[2] + rho[3]) / 60 * volume;
	        	K[6][9] = K[7][10] = K[8][11] = (rho[0] + rho[1] + 2 * rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[9][6] = K[10][7] = K[11][8] = (rho[0] + rho[1] + 2 * rho[2] + 2 * rho[3]) / 120 * volume;
	        	K[9][9] = K[10][10] = K[11][11] = (rho[0] + rho[1] + rho[2] + 3 * rho[3]) / 60 * volume;
	        }else {
	        	K[0][0] = K[1][1] = K[2][2] = (2 * rho[0] + rho[1] + rho[2] + rho[3]) / 20 * volume;
	        	K[3][3] = K[4][4] = K[5][5] = (rho[0] + 2 * rho[1] + rho[2] + rho[3]) / 20 * volume;
	        	K[6][6] = K[7][7] = K[8][8] = (rho[0] + rho[1] + 2 * rho[2] + rho[3]) / 20 * volume;
	        	K[9][9] = K[10][10] = K[11][11] = (2 * rho[0] + rho[1] + rho[2] + rho[3]) / 20 * volume;
	        }
	}

	public static void elementVolumeForce(int[] ele, double[][] xyz, double[][] f, BVPType tp, double[] eleF) {
		double volume = CG.volume3DT4(xyz);
		double[] fu = f[0];
		double[] fv = f[1];
		double[] fw = f[2];
		double sumFu = MVO.sum(f[0]);
		double sumFv = MVO.sum(f[1]);
		double sumFw = MVO.sum(f[2]);
		Arrays.fill(eleF, 0.0);
		if (tp == BVPType.COMMON) {
			for (int i = 0; i < 4; i++) {
				eleF[3 * i]     = (fu[i] + sumFu) * volume / 20.0;
				eleF[3 * i + 1] = (fv[i] + sumFv) * volume / 20.0;
				eleF[3 * i + 2] = (fw[i] + sumFw) * volume / 20.0;
			}
		}
	}
	
	public void assembleSurfaceForce(ScalarFunc[] coefFunc, double[] param, BVPType tp, double[] RHS, int... bdLabel) {
		assembleBoundaryVector(coefFunc, param, FES3T42::elementSurfaceForce, tp, RHS, bdLabel);

	}

	public void assembleSurfaceForce(double[] constCoef, BVPType tp, double[] RHS, int... bdLabel) {
		assembleBoundaryVector(constCoef, FES3T42::elementSurfaceForce, tp, RHS, bdLabel);
	}
	
    public static void elementSurfaceForce(int[] ele, double[][] xyz, double[][] f, BVPType tp, double[] eleF) {
    	double[] fu = f[0];
    	double[] fv = f[1];
    	double[] fw = f[2];
    	double volume = CG.area3DT3(xyz);
    	double sumFu = MVO.sum(fu);
    	double sumFv = MVO.sum(fv);
    	double sumFw = MVO.sum(fw);
    	for(int i = 0; i < 3; i++) {
			eleF[3 * i] =     (sumFu + fu[i]) * volume / 12;
			eleF[3 * i + 1] = (sumFv + fv[i]) * volume / 12;
			eleF[3 * i + 2] = (sumFw + fw[i]) * volume / 12;
    	}
//System.out.println(MVO.toString(ele));
//System.exit(0);
//    	ele[0] = (sumFu + fu[0]) * volume / 12;
//    	ele[1] = (sumFv + fv[0]) * volume / 12;
//    	ele[2] = (sumFw + fw[0]) * volume / 12;
    }
    
	public void assembleHeatLoad(ScalarFunc[] coef, double[] param, double[][] vecCoef, BVPType tp, double[] RHS) {
		assembleGlobalVector(coef, param, vecCoef, FES3T42::elementHeatLoad, tp, RHS);
	}

	public void assembleHeatLoad(double[] coef, double[][] vecCoef, BVPType tp, double[] RHS) {
		assembleGlobalVector(coef, vecCoef, FES3T42::elementHeatLoad, tp, RHS);
	}

	public static void elementHeatLoad(int[] ele, double[][] coord, double[][] coef, BVPType tp, double[] eleLoad) {
	    
		double[][] grad = new double[4][3];
		double volume = CG.gradient3DT4(coord, grad);
		double myCoef = 0.0;
		switch(tp) {
		case COMMON:
			double[] B = coef[0];
			double[] T = coef[1];
			myCoef = (MVO.sum(B) * MVO.sum(T) + MVO.dot_product(B, T)) * volume / 20;
			for(int i = 0; i < 4; i++) {
				eleLoad[3 * i] = myCoef * grad[i][0];
				eleLoad[3 * i + 1] = myCoef * grad[i][1];
				eleLoad[3 * i + 2] = myCoef * grad[i][2];
			}
			break;
		case CONSTITUTE_MATRIX_COMMON:
			double[] B1 = coef[0];
			double[] B2 = coef[1];
			double[] B3 = coef[2];
			double[] TT = coef[3];
			double myCoefX = (MVO.sum(B1) * MVO.sum(TT) + MVO.dot_product(B1, TT)) * volume / 20;
			double myCoefY = (MVO.sum(B2) * MVO.sum(TT) + MVO.dot_product(B2, TT)) * volume / 20;
			double myCoefZ = (MVO.sum(B3) * MVO.sum(TT) + MVO.dot_product(B3, TT)) * volume / 20;
			for(int i = 0; i < 4; i++) {
				eleLoad[3 * i] = myCoefX * grad[i][0];
				eleLoad[3 * i + 1] = myCoefY * grad[i][1];
				eleLoad[3 * i + 2] = myCoefZ * grad[i][2];
			}
			break;
		default:
			throw new IllegalArgumentException("Wrong BVPType!");
		}
	}
	

	
	public void assembleSurfacePressure(ScalarFunc[] coefFunc, double[] param, BVPType tp, double[] RHS, int... bdLabel) {
		assembleBoundaryVector(coefFunc, param, FES3T42::elementPressure, tp, RHS, bdLabel);

	}

	public void assemblePressure(double[] constCoef, BVPType tp, double[] RHS, int... bdLabel) {
		assembleBoundaryVector(constCoef, FES3T42::elementPressure, tp, RHS, bdLabel);
	}
	
	public void assemblePressure(double[][] vecCoef, BVPType tp, double[] RHS, int... bdLabel) {
		assembleBoundaryVector(vecCoef, FES3T42::elementPressure, tp, RHS, bdLabel);
	}
	
	public static void elementPressure(int[] ele, double[][] xyz, double[][] f, BVPType tp, double[] eleF) {
		double[] pressure = f[0];
		double[] normal = CG.unitOutNormal3DT3(xyz);
		double[][] ff = new double[3][3];
		for(int i = 0; i < 3; i++) {
			for(int j = 0; j < 3; j++) {
				ff[i][j] = -normal[i] * pressure[j];
			}
		}
//		f[0][0] = -pressure[0] * nx;
//		f[0][1] = -pressure[1] * nx;
//		f[0][2] = -pressure[2] * nx;
//		f[1][0] = -pressure[0] * ny;
//		f[1][1] = -pressure[1] * ny;
//		f[1][2] = -pressure[2] * ny;
//		f[2][0] = -pressure[0] * nz;
//		f[2][1] = -pressure[1] * nz;
//		f[2][2] = -pressure[2] * nz;
		elementSurfaceForce(ele, xyz, ff, tp, eleF);
		
	}
	
    public void computeStrainStressEqStress(BVPType tp, double[] coef, double[] sol, double[][] strain,
			double[][] stress, double[] misesStress) {
		int len = sol.length;
		ParamCheck.checkEqual(len, ndof);
		double[][] coord = new double[4][3];
		double[][] grad = new double[4][3];
		double[] volCount = new double[len];
		double[] eleU = new double[4];
		double[] eleV = new double[4];
		double[] eleW = new double[4];
	    double E = coef[0];
	    double Nu = coef[1];
	    double lambda =  E * Nu / ((1 - 2 * Nu) * (1 + Nu));
	    double mu = E / (2 * (1 + Nu));
	    MVO.fill(strain, 0.0);
		for(int i = 0, n = mesh.getNt(); i < n; i++) {
			int[] ele = mesh.getElement(i);
			mesh.getCoordPerElement(ele, coord);
			for(int j = 0; j < 4; j++) {
				eleU[j] = sol[3 * ele[j]];
				eleV[j] = sol[3 * ele[j] + 1];
				eleW[j] = sol[3 * ele[j] + 2];
			}
			double volume = CG.gradient3DT4(coord, grad);
			double[] dx = MVO.col(grad, 0);
			double[] dy = MVO.col(grad, 1);
			double[] dz = MVO.col(grad, 2);
			double dxU = MVO.dot_product(eleU, dx);
			double dyU = MVO.dot_product(eleU, dy);
			double dzU = MVO.dot_product(eleU, dz);
			double dxV = MVO.dot_product(eleV, dx);
			double dyV = MVO.dot_product(eleV, dy);
			double dzV = MVO.dot_product(eleV, dz);
			double dxW = MVO.dot_product(eleW, dx);
			double dyW = MVO.dot_product(eleW, dy);
			double dzW = MVO.dot_product(eleW, dz);
			for(int j = 0; j < 4; j++) {
				strain[0][ele[j]] += dxU * volume;      
				strain[1][ele[j]] += dyV * volume;
				strain[2][ele[j]] += dzW * volume;
				strain[3][ele[j]] += (dxV + dyU) * volume;
				strain[4][ele[j]] += (dyW + dzV) * volume;
				strain[5][ele[j]] += (dzU + dxW) * volume;
				volCount[ele[j]] += volume;
			}
		}
		
		for(int i = 0, n = mesh.getNv(); i < n; i++) {
			strain[0][i] /= volCount[i];
			strain[1][i] /= volCount[i];
			strain[2][i] /= volCount[i];
			strain[3][i] /= volCount[i];
			strain[4][i] /= volCount[i];
			strain[5][i] /= volCount[i];
		}
		
		for(int i = 0, n = mesh.getNv(); i < n; i++) {
		    stress[0][i] = lambda * (strain[1][i] + strain[2][i]) + (lambda + 2 * mu) * strain[0][i];
		    stress[1][i] = lambda * (strain[0][i] + strain[2][i]) + (lambda + 2 * mu) * strain[1][i];
		    stress[2][i] = lambda * (strain[0][i] + strain[1][i]) + (lambda + 2 * mu) * strain[2][i];
		    stress[3][i] = mu * strain[3][i];
		    stress[4][i] = mu * strain[4][i];
		    stress[5][i] = mu * strain[5][i];
			double sxy = stress[0][i] - stress[1][i];
			double syz = stress[1][i] - stress[2][i];
			double sxz = stress[0][i] - stress[2][i];
			misesStress[i] = Math.sqrt(0.5 * (sxy * sxy + syz * syz + sxz * sxz) 
					       + 6 * (stress[3][i] * stress[3][i] + stress[4][i] * stress[4][i] + stress[5][i] * stress[5][i]));
		}

    }

}
